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Summary.  In  this  review  we  explore  the  possibility  of  adapting  first  order  hybrid 
feedback  controllers  for  nonholonomically  constrained  systems  to  their  dynamical 
counterparts.  For  specific  instances  of  first  order  models  of  such  systems,  we  have 
developed  gradient  based  hybrid  controllers  that  use  Navigation  functions  to  reach 
point  goals  while  avoiding  obstacle  sets  along  the  way.  Just  as  gradient  controllers 
for  standard  quasi-static  mechanical  systems  give  rise  to  generalized  “PD-style” 
controllers  for  dynamical  versions  of  those  standard  systems,  so  we  believe  it  will 
be  possible  to  construct  similar  “lifts”  in  the  presence  of  non-holonomic  constraints 
notwithstanding  the  necessary  absence  of  point  attractors. 


1  Introduction 

The  use  of  total  energy  as  a  Lyapunov  function  for  mechanical  systems  has  a 
long  history  [1]  stretching  back  to  Lord  Kelvin  [2].  Unquestionably,  Arimoto 
[3]  represents  the  earliest  exponent  of  this  idea  within  the  modern  robotics 
literature,  and,  in  tribute  to  his  long  and  important  influence,  we  explore 
in  this  paper  its  extension  into  the  realm  of  nonholonomically  constrained 
mechanical  systems. 

The  notion  of  total  energy  presupposes  the  presence  of  potential  forces 
arising  from  the  gradient  of  a  scalar  valued  function  over  the  configuration 
space.  We  focus  our  interest  on  “artificial  cost  functions”  introduced  by  a 
designer  to  encode  some  desired  behavior  as  originally  proposed  by  Khatib 
[4,  5].  However,  we  take  a  global  view  of  the  task,  presuming  a  designated 
set  of  prohibited  configurations  —  the  “obstacles”  —  and  a  designated  set 
of  selected  configurations  —  the  “goal,”  which  we  restrict  in  this  paper  to 
be  an  isolated  single  point.  We  achieve  the  global  specification  through  the 
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introduction  of  a  Navigation  Function  (NF)  [6]  an  artificial  potential 
function  that  attains  a  maximum  value  of  unity  on  the  entire  boundary  of  the 
obstacle  set,  and  its  only  local  minimum,  at  zero,  exactly  on  the  isolated  goal 
point.  Such  functions  are  guaranteed  to  exist  over  any  configuration  space  of 
relevance  to  physical  mechanical  systems  [7],  and  constructive  examples  have 
been  furnished  for  a  variety  of  task  domains  [6,  8,  9,  10]. 

NF-generated  controls  applied  to  completely  actuated  mechanical  systems 
force  convergence  to  the  goal  from  almost  every  initial  condition  and  guarantee 
that  no  motions  will  intersect  the  obstacle  set  along  the  way.  In  the  dynamical 
setting,  where  the  role  of  kinetic  energy  is  important,  they  achieve  a  pattern  of 
behavior  analogous  to  that  of  similarly  controlled  corresponding  quasi-static 
dynamics.  For  example,  in  the  one  degree  of  freedom  case,  the  dynamical 
setting  is  represented  by  the  familiar  spring-mass-damper  system 

mq  +  cq  +  kq  =  0  (1) 

and  the  corresponding  quasi-static  model  arises  through  a  neglect  of  the  in¬ 
ertial  forces,  in  — >  0  in  (1),  yielding 

cq  +  kq  =  0  (2) 

To  illustrate  the  nature  of  NF-gradient-basecl  controllers  in  this  simple 
setting,  take  the  configuration  space  to  be  Q:={g€R:|<7|<l}  with  nav¬ 
igation  function  ip(q)  :=  \kq2 ,  implying  that  {0}  =  is  the  goal  and 

{—1,1}  =  ^_1[1]  the  obstacle  set.  We  imagine  that  both  systems,  (1),  (2), 
arise  from  application  of  the  NF-gradient  control  law,  u  :=  —  Nip,  to  the  re¬ 
spective  open  loop, 


u  =  mq  +  cq 
or 

u  =  cq. 

We  observe  that  ip  is  a  global  Lyapunov  function  for  (2)  guaranteeing  that 
all  initial  conditions  give  rise  to  motions  that  avoid  the  obstacle  set  while 
converging  asymptotically  on  the  goal  set.  Analogously,  the  total  energy, 
/i  :=  \q2  +  (p(q)  is  a  Lyapunov  function  for  the  velocity- limited  extension  of 
Q,  X  :=  /u,- 1  [0, 1]  =  {'(q,  q)  €  M2  :  p(q,q)  <  l}.  This  guarantees  that  all  ini¬ 
tial  conditions  in  X  give  rise  to  motions  that  avoid  the  obstacle  (and,  in  fact, 
are  repelled  from  the  entire  boundary,  1  [1])  while  converging  asymptoti¬ 
cally  on  the  zero  velocity  goal  set,  p_1[0]  =  {(0,0)}.  A  more  general  global 
version  of  Arimoto’s  [3]  adaptation  of  Lord  Kelvin’s  observations  has  been 
presented  in  greater  detail  in  [11]. 

In  contrast,  for  incompletely  actuated  mechanical  systems,  when  the  de¬ 
grees  of  freedom  exceed  the  number  of  independently  controlled  actuators, 
the  applicability  of  NF-gradient-basecl  controllers  to  either  dynamical  or 
quasi-static  mechanical  systems  remains  largely  unexplored.  One  particu¬ 
larly  important  class  of  such  systems  arises  in  the  presence  of  nonholonomic 
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constraints  —  systems  with  intrinsically  unavailable  velocities  whose  absence 
cannot  be  expressed  in  terms  of  configuration  space  obstacles  [12].  In  such  set¬ 
tings,  there  is  an  inherent  degree  of  underactuation,  since  no  amount  of  input 
work  can  be  exerted  in  the  forbidden  directions.  In  an  echo  of  Arimoto’s  [3] 
“lift”  of  first  order  gradient  dynamics  (2)  to  damped  second  order  mechanical 
dynamics  (1),  this  paper  explores  the  relationship  between  quasi-static  and 
fully  dynamical  NF-gradient  controllers  for  a  class  of  nonholonomic  systems. 

One  very  important  general  observation  about  nonlinear  systems  that 
throws  a  shadow  on  every  aspect  of  this  exploration  was  made  two  decades  ago 
by  Brockett  [13]  who  pointed  out  that  nonlinear  systems  may  be  completely 
controllable  while  failing  to  be  smoothly  stabilizable.  Nonholonomically  con¬ 
strained  systems  suffer  this  defect,  so  that  no  smooth  feedback  controller, 
NF-gradient  or  otherwise,  could  ever  stabilize  a  single  point  goal.  However, 
switching  controllers  incur  no  such  limitation. 

In  the  next  section  we  will  introduce  a  switching  controller  for  a  broad 
class  of  systems  that  alternately  runs  “down”  and  then  “across”  the  gradient 
slope  to  bring  all  motions  arbitrarily  close  to  the  goal  without  hitting  any 
obstacles.  For  these  classes  we  can  prove  this  analytically.  The  next  section 
addresses  the  same  class  of  systems,  now  cast  in  the  dynamical  setting.  We 
show  how  to  recast  the  hybrid  controller  to  give  the  analogous  result  for  these 
second  order  systems.  Finally,  as  an  illustrative  example,  we  present  numerical 
simulations  of  the  rolling  disk  defined  in  a  configuration  space  with  a  simple 
sensory  model  that  imposes  a  particular  configuration  space  topology. 


2  Hybrid  Controller  for  Nonholonomic  Kinematic 
Systems 

We  start  by  presenting  a  class  of  controllers  defined  in  R3  for  nonholonomic 
kinematic  systems.  For  an  in  depth  exposure  please  see  [14].  Consider  the  class 
of  smooth  and  piecewise  analytic,  three  degree  of  freedom,  drift-free  control 
systems 


q  =  B(q)u,  q  G  Q  C  R3;  u  G  R2,  (3) 

where  Q  is  a  smooth  and  piecewise  analytic,  compact,  connected  three  di¬ 
mensional  manifold  with  a  boundary,  dQ  (that  separates  the  acceptable  from 
the  forbidden  configurations  of  R3),  possessing  a  distinguished  interior  goal 
point ,  q*  G  Q.  In  this  section  we  will  impose  very  general  assumptions  on 
B  and  construct  a  hybrid  controller  that  guarantees  local  convergence  to  an 
arbitrarily  small  neighborhood  of  the  goal  state  while  avoiding  any  forbidden 
configurations  along  the  way.3 


3  In  the  next  section,  we  will  introduce  more  specialized  assumptions  that  extend 
the  basin  of  attraction  to  include  almost  every  initial  configuration  in  Q. 
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We  find  it  convenient  to  write  (3)  using  the  nonholonomic  projection  ma¬ 
trix  [15],  H  into  the  image  of  B: 

H(q)  =  B(q)B(q)l  =  B(q)  (B(q)TB(q)) _1  B(q)T  (4) 

q  =  H(q)v,  qe  Q  Cl3;  »£l3  (5) 

2.1  Two  Controllers  and  Their  Associated  Closed  Loop  Dynamics 

It  is  useful  to  compare  the  unconstrained  system  q  =  v  with  the  constrained 
version  (5).  Let  ip  be  a  navigation  function  defined  in  Q.  For  the  input 
v  =  —  S7p  the  unconstrained  system  is  globally  asymptotically  stable  at  the 
origin.  Using  p  as  a  control  Lyapunov  function  yields  p  =  — ||V(^||2.  Given 
this  result,  a  naive  approach  to  attempt  stabilizing  system  (5)  is  to  use  the 
same  input  v  =  —Vp.  Define  the  vector  held  /i  :  Q  — >  TQ  such  that 
fi(q)  :=  —H{q)\/p(q)  and  the  system 

q  =  fi(q)  =  ~H(q)Vp(q)  (6) 

Since  H  has  a  1-dimensional  kernel  it  follows  that  (6)  has  a  1  dimensional 
center  manifold 


Wc  :=  {qeQ:H(q)X7p(q)  =  0}  , 


as  corroborated  by  explicitly  computing  the  Jacobian  of  /i  at  q*: 


Dh \p 


-(VpL.  ®I)DHS  -  HD2 p 


=o 


HD2p 


(7) 


Using  p  as  a  control  Lyapunov  function,  La  Salle’s  invariance  theorem  states 
that  system  (6)  has  its  limit  set  in  >VC: 


p  =  —\7pTH\7p 


=  -||tfv^||2 


=  0  if  q  G  Wc 
<  0  if  q  i  Wc 


(8) 


Figure  1  illustrates  the  topology  associated  with  (6):  the  projection  H  im¬ 
poses  a  co-dimension  1  foliation  complementary  to  the  center  manifold.  The 
stable  manifold,  Ws,  is  the  leaf  containing  the  goal,  q* .  The  input 

ui  :=  B(q)^p(q)  (9) 


alone  cannot  stabilize  system  (6)  at  the  origin,  since  no  smooth  time  invariant 
feedback  controller  has  a  closed  loop  system  with  an  asymptotically  stable 
equilibrium  point  [12].  Nevertheless,  for  any  initial  condition  outside  Wc  an 
infinitesimal  motion  in  the  direction  of  f±  reduces  the  energy  p.  If  there  can 
be  found  a  second  controller  that  “escapes”  >VC  without  increasing  p  then  it 
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Fig.  1.  Conceptual  illustration  of  the  flow  associated  with  (6).  Each  leaf  is  an 
invariant  manifold  with  all  trajectories  collapsing  into  Wc. 


is  reasonable  to  imagine  that  iterating  the  successive  application  of  these  two 
controllers  might  well  lead  eventually  to  the  goal.  We  now  pursue  this  idea  by 
introducing  the  following  controller, 

u2  ■=  B(qY  [A(q)  x  Vip(q)] ,  (10) 

leading  to  the  closed  loop  vector  field 

Q  =  Mo) 

f2(q)  :=  A(q)  x  \7 tp(q)  (11) 

where  A(q)  :=  xB(q)  is  the  cross  product  of  the  columns  of  B(q).  4  Note 
that  the  nonholonomic  constraint  expressed  in  (3)  can  be  represented  by  the 
implicit  equation  AT  (q)q  =  0.  Since  the  derivative  of  <p  in  the  direction  of  f2 
is 

Lf2<p  =  S7ip{q)T  •  ( A(q )  x  v<p(q))  =  0,  (12) 

it  follows  that  f2  is  ^-invariant  —  i.e.  the  energy,  ip,  is  constant  along  its 
motion.  Moreover  0  =  AT(A  x  V^j)  =  AT f2,  verifying  that  f2  indeed  satisfies 
the  constraint  (3). 

We  will  assume  in  A3  that  B  has  rank  two  at  each  point. 


4 
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2.2  Assumptions,  a  Strategy,  and  Preliminary  Analysis 

Given  the  previous  two  vector  fields  —  one  which  is  energy  decreasing;  the 
other  energy  conserving  —  we  now  sketch  a  strategy  that  brings  initial  condi¬ 
tions  of  system  (3)  to  within  an  arbitrarily  small  neighborhood  e  of  the  goal, 
by  way  of  motivating  the  subsequent  definitions  and  claims  that  arise  in  the 
formal  proofs  to  follow.  Let  ff(q)  and  /|(g)  denote  the  flows  of  fi  and  /2 
respectively. 

(1) .  If  qo  £  Wc  then  follow  a  direction  in  im (H)  for  a  finite  amount  of  time 

to  such  that  f{°(qo)  £  Wc  and  p  o  f[°(qo)  <  1  for  all  t  €  (0,  t0). 

(2) .  If  q0  ^  >VC  and  p(qo)  >  e 

2.1)  Use  a  scaled  version  of  /2  for  time  r2  to  escape  a  ^-neighborhood  of 
Wc,  keeping  the  energy  p  constant. 

2.2)  Use  controller  /i,  for  time  n,  to  decrease  the  energy  p,  stopping  at  a 
y-neighborhood  of  >VC  such  that  / f1  (q)  £  Wc  and  7  <6. 

We  now  introduce  a  number  of  assumptions,  definitions  and  their  consequences 
that  will  allow  us  to  formalize  each  of  the  previous  steps: 

A1  Q  is  a  smooth  compact  connected  manifold  with  boundary. 

A2  p  is  a  navigation  function  in  Q. 

A3  H  has  rank  two,  uniformly  throughout  Q. 

Assumption  A1  gives  the  proper  setting  for  the  existence  of  a  naviga¬ 
tion  function  in  the  configuration  space. Assumption  A3  assures  the  foliation 
sketched  in  figure  1. 

Define  the  local  surround  of  the  goal  to  be  the  closed  “hollow  sphere”, 
Qs  :=  p~1[<hs\,  with  <hs  :=  je,  <ps]  whose  missing  inner  “core”  is  the  arbitrar¬ 
ily  small  open  neighborhood,  Qe  :=  G>t  :=  [0,e),  and  whose  outer 

“shell”,  Qi  :=  with  $1  :=  (<yjs,l],  includes  the  remainder  of  the  free 

configuration  space.  ps  is  defined  to  be  the  largest  level  such  that  all  the 
smaller  levels,  po  £  (0,</?s)  are  homeomorphic  to  the  sphere,  S'2,  and  are  all 
free  of  critical  points,  ||V<p||-1[0]  fl  p~x  [(0,  <ps)]  =  0. 

The  restriction  to  (^-invariant  topological  spheres  precludes  limit  sets  of  /2 
more  complex  than  simple  equilibria  in  the  local  surround.  However,  has  in  the 
examples  section  of  [14],  one  can  provide  more  specialized  conditions  resulting 
in  the  guarantee  that  the  algorithm  brings  almost  every  initial  condition  in 
the  “outer”  levels,  Qi  into  the  local  surround,  Qs  and,  thence,  into  the  goal 
set  Qe. 

Lemma  1  ([14]).  Given  the  previous  assumptions 

/rM 0]  n  Qs  =  /2-x[ o]  n  Qs  =  wc  n  Qs.  (13) 
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To  formally  express  the  “^-neighborhood”  described  in  the  stabilization 
strategy  we  start  by  defining  the  function  £  :  Q  —  {<7*}  — >  [0, 1]: 


£(g)  “ 


|jg(g)vy(g)ll2 

||V^(g)||2 


(14) 


The  quantity  ||i4(g)V(/?(g)||2  evaluates  to  zero  only  in  >VC.  Therefore  in 
a  small  neighborhood  of  Wc  the  level  sets  of  \\H (q)\7 tp(q)\\2  define  a  “tube” 
around  >VC.  The  denominator  of  (14)  normalizes  £  such  that  0  <  £  <  1. 
Moreover  it  produces  a  “pinching”  of  the  tube  at  the  goal  q*. 

Lemma  2  ([14]).  For  all  £<FS  ,  ip~l[ipo\  intersects  the  unit  level  set  of  £, 
i.e.,  £-1[l]  n</?-1[v?0]  0. 

Corollary  1  ([14]).  For  all  tp0  £  <PS  the  level  set  <p  1[v?o]  intersects  every 
level  set  of  £,  i.e.,  £_1[a]  D  1  [«po]  7^  0  for  all  a  £  [0, 1]. 

Lemma  3  ([14]).  A  sufficient  condition  for  the  Jacobian  of  /2(g)  evaluated 
at  Wc  —  ||V</j||_1[0]  to  have  at  least  one  eigenvalue  with  non-zero  real  part  is 
that  the  control  Lie  algebra  on  B  spans  R3. 


Lemma  4  ([14]).  The  Jacobian  of  /2(g)  evaluated  at  Wc  fl  Qs  has  two  non¬ 
zero  real  part  eigenvalues  with  the  same  sign. 


Now  consider  the  implicit  equation, 


£(g)=£*^||ir(9)VV(g)||2  =  £lV^(«)||2 


(15) 


At  the  goal  any  £*  satisfies  (15).  Although  £  is  not  defined  at  q*  all  of  its  level 
sets  intersect  at  q* . Finally,  define  the  parameterized  cone  C7  around  Wc,  and 
its  complement  Cf  :=  Q  —  C7  —  {g*},  by: 

C7  =  {q  £  Q-  {<?*}  :  £(<?)  <  7}  (16) 

We  follow  by  imposing  conditions  on  H  and  A  such  that  the  vector  held  /2 
can  afford  the  needed  “escape”  from  >VC. 

Lemma  5  ([14]).  Suppose  system  (3)  satisfies  assumptions  A1-A3  and,  hence, 
the  previous  lemmas.  Then,  there  exists  a  function  a  :  Q  — >  R  that  renders 
the  system 

g  =  v(q)A(q)  x  Vip(q)  =  /2(g)  (17) 


unstable  at  Wc  fl  Qs. 

Corollary  2  ([14]).  Under  the  conditions  of  the  previous  lemma,  there  can 
be  found  a  r  £  (0,  00)  such  that,  for  all  go  £  £^1[<5/2]  we  have  £  o  /J (go)  >  6. 

Figure  2  illustrates  the  steps  used  in  the  previous  proof.  Trajectories  start¬ 
ing  inside  N  —  Cf  will  traverse  dC7  and  dCs  in  finite  time. 
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Wc 


Fig.  2.  Illustration  of  the  construction  used  in  the  proof  of  corollary  2. 


2.3  A  Hybrid  Controller  and  Proof  of  its  Local  Convergence 

Given  the  previous  result  define  the  time  variables  T\ ,  r2  and  the  scalars  7  <  6 
such  that: 


n(q,  7) 
Mq,  S) 


/  min  {t  >  0  |  £(/f(g))  =  7} 

\o 

fmin{t  >  0  |  £(/j(g))  =  5} 

1  0 


if  ?eq 

otherwise 

if  q  €  Cs  -  Wc 
otherwise 


I.e.,  r 1  is  the  time  to  reach  the  7  neighborhood  of  Wc  using  vector  held  /1 
and  72  is  the  time  to  escape  from  a  7  neighborhood  to  a  6  neighborhood  of 
Wc  using  vector  held  /2. 

This  results  in  the  following  maps: 

/r  :  C*  ac7  (18) 

f?  :  Qs-  wc  ^  Cf  C  gc,  (19) 

where  C  is  the  closure  of  C.  With  6  =  2 7  dehne  the  map  P  :  Qs  —  Wc  — >  dC7 
P(q)  =  fI1^)of?^\q)  (20) 


and  consider  the  recursive  equation: 


gfc+i  =  P(gfe). 


(21) 


The  set  dC1  can  be  interpreted  as  a  Poincare  section  for  the  discrete  system 
(21).  We  are  now  ready  to  present  the  following  result: 

Theorem  1  ([14]).  There  exists  an  iteration  number,  N  :  Qs  — >  N  such  that 
the  iterated  hybrid  dynamics,  PN  brings  Qs  to  Qe. 
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Proof.  Define 


N  :=  min  { n  €  N|0  <  N  <  Ne\ip  o  Pn(qo)  <  e}  , 


and  Atp(q)  :=  ip  o  P{q)  —  p(q).  Since  Qs  is  a  compact  set  it  follows  that 
\Ap\  achieves  its  minimum  value,  Ae,  on  that  set,  hence  at  most  Ne  := 
ceiling(ps  —  e)/Z\e  iterations  are  required  before  reaching  Qe. 


Note  that  all  initial  conditions  in  the  pre-image  of  the  “local  surround”, 
1Z  :=  Ut>o  /f  «2»-Wc)  are  easily  included  in  the  basin  of  the  goal,  Qs,  by  an 
initial  application  of  the  controller  u\ .  While  it  is  difficult  to  make  any  general 
formal  statements  about  the  size  of  7Z ,  we  show  in  the  next  section  that  for 
all  the  examples  we  have  tried,  the  “missing”  initial  conditions,  Q  —  7Z  =  Z, 
comprise  a  set  of  empty  interior  (in  all  but  one  case  Z  is  actually  empty) 
because  all  of  W’c,  excepting  at  most  a  set  of  measure  zero,  is  included  in  Qs. 
In  configuration  spaces  with  more  complicated  topology,  there  is  no  reason  to 
believe  that  this  pleasant  situation  would  prevail.  To  summarize,  the  following 
algorithm  is  guaranteed  to  bring  all  initial  configurations  in  1Z  to  the  goal  set, 

(1) .  \/q0  €  Qs  —  Wc,  follow  successive  applications  of  (21),  i.e.  use  the  inputs 

to  equation  (3): 

ui(q)  ■=  B\q)V<p(q)  (22) 

u2{q)  ■=  a(q)B'<(q)J(A(q))\/(p(q)  (23) 

(2) .  Vgo  €  Wc  use  the  input 


u3  := 


(24) 


for  a  small  amount  of  time  t.3  such  that  ip  o  fff  (qo)  <  1,  with  f3(q)  := 
B(q)u3. 

(3).  Vg0  G  7 Z—  Qs,  use  the  input  u\  for  time  t  until  fl(qo)  £  Qs- 


2.4  Other  Considerations 

•  Limit  cycles  in  the  level  sets  of  ip.  In  many  practical  applications 
switching  between  controllers  /i  and  /2  using  a  small  ^-neighborhood  is 
far  too  conservative.  It  may  be  possible  to  escape  >VC  by  more  than  just  the 
small  collar  £-1[<5].  If  we  could  recognize  the  passage  into  Ws  and  switch 
off  controller  u2  (i.e.  turn  Ws  into  an  attractor  of  a  suitable  modified  form 
of  /2)  then  a  final  application  of  controller  u\  is  guaranteed  to  achieve  the 
goal  state,  q* .  The  hope  of  reworking  the  form  of  u2  so  that  the  resulting 
closed  loop  vector  field,  /2,  has  its  forward  limit  set  solely  in  >VS  thus 
raises  the  question  of  when  there  exists  limit  cycles  in  the  level  sets  of  p 
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for  the  flow  of  fa.  More  importantly,  we  seek  a  condition  that  guarantees 
that  every  trajectory  of  fa  starting  in  a  small  neighborhood  of  Wc  can 
intersect  Ws  either  by  forward  or  inverse  time  integration  of  system  (11). 
Note  that  fa  generates  a  planar  flow,  making  the  Bendixon’s  criteria  a 
natural  candidate  for  such  condition.  Several  authors  [16,  17,  18,  19]  have 
developed  extensions  to  Bendixson’s  criteria  for  higher  dimensional  spaces, 
obtaining  in  general  conditions  that  preclude  invariant  sub-manifolds  on 
some  set.  For  systems  with  first  integrals,  such  as  some  classes  of  systems 
that  result  from  nonholonomic  constraints,  the  conditions  simplify  to  a  di¬ 
vergence  style  test.  Feckan’s  theorem  (see  [16])  states  that  in  open  subsets 
where  div/2  fa  0  there  can  exist  no  invariant  submanifolds  of  any  level 
precluding  cyclic  orbits.  The  divergence  measure  can  be  used  this  way  to 
detect  limit  cycles.  Note  however,  that  the  previous  result  does  not  pre¬ 
clude  quasi-periodic  orbits. 

•  Computational  heuristic  substitutes  for  a.  The  a  function  intro¬ 
duced  in  Lemma  5  modifies  the  flow  of  fa  rendering  the  center  manifold 
unstable.  Having  that  property  is  sufficient  for  stabilization,  but  more 
can  be  accomplished.  By  careful  craft  of  cr  one  can  minimize  the  number 
of  switches  between  controllers  fa  and  fa  necessary  to  reach  the  desired 
neighborhood  of  the  goal.  If  the  stable  manifold  >VS  is  contained  in  the 
zero  set  of  a  and  Ws  is  made  attractive  by  fa  for  any  point  in  Qs  then  one 
gets  f£°  o  f£°(Qs)  =  q*,  i.e.,  only  2  steps  are  necessary  to  reach  the  goal. 
Different  methods  for  approximating  <7  are  presented  in  [14].  Specifically 
the  function  a  is  replaced  by  the  divergence  of  fa  in  a  neighborhood  of 
Wc;  by  maximizing  £,  since  that  implies  escaping  Wc  in  some  measure; 
or  replacing  a  by  and  implicit  polynomial  stable  manifold  approximation 
(please  see  [20,  14]  for  invariant  manifold  computations). 


3  Hybrid  Controller  for  Nonholonomic  Dynamic  Systems 

In  this  section  we  look  into  the  “lift”  of  the  algorithm  proposed  in  the  previ¬ 
ous  section  to  nonholonomic  ally  constrained  dynamical  systems.  The  resulting 
corollaries  arise  naturally  from  the  ideas  introduced  in  [21].  Let  (25)  and  (26) 
be  the  system  of  equations  for  unconstrained  systems  [22]  and  nonholonomi- 
cally  constrained  systems  [23]  with  q,u  €  R"  and  v  £  Rm,  m  <  n: 


M{q)q  +  c(q,q)  =  u 

(25) 

M(q)q+c(q,q)  =  A(q)T  X  +  B(q)v 

(26) 

A(q)q  =  0 

where  M  is  the  mass  matrix,  c  the  Coriolis  term,  A  and  B  represent  the  actu¬ 
ation  constraints  defined  in  section  2  and  A  is  a  vector  of  Lagrange  multipliers. 
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We  start  by  recalling  some  notation  and  lemmas  required  for  the  subsequent 
proofs.  Using  the  “stack-kronecker  notation”  [24,  25,  26]  consider  the  following 
linear  map: 


Mq  :  x  i->  [a;  0  I]T DqMs  (27) 

and  the  skew-symmetric  value  operator: 

Jq{x)  :=  Mq(x)  —  Mq(x)  (28) 

Lemma  6  ([21]).  For  any  curve,  q  :  M  — *  Q,  and  any  vector,  x  G  Tqito)Q, 

Mq\tqX  =  Mg(t0)(x)q\t0  (29) 

Lemma  7  ([21]).  Given  a  Lagrangian  with  kinetic  energy,  n,  with  no  poten¬ 
tial  forces  present,  and  with  an  external  torque  or  force  actuating  at  every 
degree  of  freedom  as  specified  by  the  vector,  t,  the  equations  of  motion  may 
be  written  in  the  form: 


M(q)q  + c(q,q)  =  t  (30) 

where 

c(q,x)  =  C(q,x)x  (31) 

and 

C(q,x)  :=^M(qx)  (32) 

Notice  that  the  representation  of  the  Coriolis  and  centripetal  forces  in 
terms  of  the  bilinear  operator  valued  map  C  only  coincide  at  q  with  the 
quadratic  expression  c(q,q).  In  general  they  are  not  the  same. 

Corollary  3  ([21]).  For  any  motion  q  :  R  — >  Q,  and  any  tangent  vector, 
x  G  TQq(t), 


-T\  -2M(q)-C(q,q) 


x  =  0 


Proof.  From  the  previous  lemma, 


l-M{q)-C{q,q) 


x  =  -~xTJq(q)x  =  0 


(33) 


146  Gabriel  A.  D.  Lopes  and  Daniel  E.  Koditschek 

3.1  Embedding  the  Limit  Behavior  of  Gradient  Dynamics 

Controller  f±(q)  =  — H{q )  •  \7ip{q),  introduced  in  Section  2.1,  aims  to  reach 
a  fixed  point  in  the  center  manifold  Wc.  In  order  to  lift  the  controller  into 
a  2nd  order  system,  theorem  2,  concerning  limit  sets  of  gradient  dynamics, 
is  complemented  with  corollary  4.  Let  the  state  variables  pi,P2  represent  q,  q 
respectively  and  let  V  =  TQ  be  the  tangent  bundle  of  Q  for  system  (25). 

Theorem  2  (Koditschek[21]).  Let  p  be  a  Morse  function  on  Q  which  is 
exterior  directed  on  the  boundary  dQ,  surpasses  de  value  p  >  0  on  the  bound¬ 
ary,  and  has  a  local  minima  at  the  points  Q  :=  {gi}"=1  C  Q.  Let  K2  >  0 
denote  some  positive  definite  symmetric  matrix.  Consider  the  set  of  ‘‘bounded 
total  energy”  states 


Pi 

P2 


G  V  :  ip(pi)  +  -P2MP2  <  p 


Under  the  feedback  algorithm 

u  :=  -K2P2  -  D(pT(pi) 


(34) 


(35) 


V M  is  a  positive  invariant  set  of  the  closed  loop  dynamical  system  within  which 
all  initial  conditions  excluding  a  set  of  measure  zero  take  Q  as  their  positive 
limit  set. 

Let  H  be  the  nonholonomic  projection  matrix.  Define  Q(q)  :=  I  —  H{q ) 
to  be  the  nonholonomic  converse  projection  matrix.  Notice  that  ker(A)  = 
ker{Q)  and  therefore  Q{q)q  =  0.  Let  Wg  :=  {q  G  >VC  A  q  =  0}.  As  shown  in 
Section  2.1,  >VC  is  the  center  manifold  of  the  system  q  =  —H{q)  ■  Vy>(g). 
Rewriting  equation  (26)  with  a  new  input  v  :=  B(qyu  we  get: 

M(q)q  +  c{q ,  q)  =  A(q)T X  +  H(q)u 

A{q)q  =  0  (36) 

Corollary  4.  Let  K2  =  K2H(q)  with  K2  >  0  denoting  a  positive  definite 
symmetric  matrix.  Under  the  conditions  of  theorem  2  all  the  initial  conditions 
of  the  system  (36),  excluding  a  set  of  measure  zero,  take  W(j  as  their  positive 
limit  set. 

Proof.  Let  V  =  (p(q)  +  ^gT M(q)q  be  a  Lyapunov  function  for  (5).  Then 

V=Dipq  +  \ qTMq  -  qT  ( HK2q  +  HDtpT  +  HCq)  +  qT AT A 

=0 

=DtpQq  +  qT  Jqq -qTHK2q 
=0  =0 

=-qTHTK2Hq 
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HT K2H  is  a  semi-definite  positive  matrix.  V  is  null  when  either  q  =  0  or 
Hq  =  0.  Since  A(q)q  =  0  =>  Hq  ^  0  then  the  largest  invariant  set  is  the 
interception  of  the  previous  sets  with  Wq  resulting  in  Wq.  La  Salle’s  theorem 
guarantees  that  (5)  with  input  (35)  takes  Wq  as  the  forward  limit. 

3.2  Embedding  of  More  General  Dynamics 

We  now  seek  to  lift  the  controller  ,/2  (<z)  defined  in  Section  2.1  to  a  2nd  order 
system.  We  do  so  by  adding  once  again  a  level  regulator  term,  so  that  the 
reference  dynamics  attracts  to  a  particular  level  set.  First  recall  the  embedding 
of  general  reference  dynamics:  let  /  be  a  reference  vector  held  with  Lyapunov 
function  /i,  and  let  F(p)  :=  P2  —  f(pi)-  Consider  the  control  algorithm, 

u  =  -K2F  -  DpT  +  MDfp2  +  Cf  (37) 

which  applied  to  the  mechanical  system  (25)  yields  a  closed  loop  form,  p  = 

hip), 


hip)  ■= 


P  2 

Dfp2  -  M~l  [K2F  +  CF  +  DpT] 


Theorem  3  (Koditschek[21]).  If  p  is  a  strict  Lyapunov  function  for  f  on 
Q,  then 


V  :=  p  +  ^ FTMF 

is  a  strict  Lyapunov  function  for  h  on  V. 

In  system  (5)  the  set  of  images  of  H  for  each  point  on  Q  is  the  tangent 
bundle  of  Q.  Therefore,  since  H  is  a  projection  operator  then  Va;  G  V  we  have 
H(px).x  =  x.  Define  H  =  M~1HM  and  Q  =  M~XQM.  For  system  (36)  with 
input  (37)  the  closed  loop  is  written  in  the  following  way: 


h{P)  ■= 


P  2 

HDfp2  -  M -1  [HI<2F  +  HCF  +  HDpT  +  QCp2  -  AT A] 


Corollary  5.  Suppose  Hf  =  f,  i.e.  the  reference  vector  field  respects  the 
nonholonomic  constraints.  If  p  is  a  strict  Lyapunov  function  for  f  on  Q  for 
system  (36),  then 


V  :=  p  +  ^ FTMF 

is  a  strict  Lyapunov  function  for  h  on  V. 

Proof.  First  note  that  Q.f  =  0;  Q.P2  =  0;  A.f  =  0. 
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V=Dp  p2  +  X-FtMF  +  FTMQDfp2  + 

-Ft  ( HK2F  +  HCF  +  HDpT  +  QCp2 )  +  FTAT\ 


=DpHf  -  FtHK2F  -  fT  Q(M  D  f  q  —  Cf)  +  fTAT_\ 

=o  =o 

=Dpf  -  FtI<2F 


According  to  the  hypothesis,  the  first  term  is  negative  except  on  the  largest 
invariant  set  of  F  =  0.  The  second  term  is  always  negative  except  in  F_1[0]. 
The  interception  of  the  two  results  in  the  limit  set  of  q  =  f(q). 

The  previous  result  show  that  as  long  as  the  reference  dynamics  respects 
the  nonholonomic  constraints  we  can  apply  Theorem  3  directly.  Notice  that 
Corollary  5  also  applies  to  controller  —H(q).W<p(q).  In  general  such  restrictive 
dynamics  are  not  necessary  for  that  controller,  so  using  Corollary  4  gives  a 
better  tool  since  we  are  only  interested  on  the  limit  set. 


4  Simulations 

In  this  section  we  present  simulation  examples  for  the  unicycle  or  vertical 
rolling  disk  depicted  in  Figure  3.  The  unicycle  is  commonly  defined  in  the 
SE(2)  configuration  space  with  constraint  equation  i;sin(6l)  —  ycos(9)  =  0. 
Since  we  are  interested  in  simulations  in  a  dynamic  setting  we  will  follow 
instead  Bloch’s  vertical  rolling  disk  [27],  defined  in  the  configuration  space 
Q  =  K2  x  S'1  x  S'1  =  SE(2)  x  S'1  with  coordinates  q  =  (x,  x,  9,  (j>).  The  equations 
of  motion  for  the  vertical  rolling  disk  are: 


Fig.  3.  The  vertical  rolling  disk. 
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(■ mR 2  +  I)<j>  =  U\ 

je  =  u2,  (38) 

with  the  constraint  equations: 

x  =  Rcos(x)<p 

y  =  Rsin(x)<j>.  (39) 

Differentiating  (39)  in  time  and  replacing  (f>  from  (38),  one  obtains  a  complete 

set  of  equations  of  motion  that  verifies  the  nonholonomic  constraints  for  initial 
conditions  that  also  verify  (39): 

RcOs(6) 

x  =  —  Rsm(9)6<f>  H - 5 - ~ui 

mR  + 1 

y  =  Rcos{0)9(j)  +  RS^°\ui  (40) 

mR  + 1 

This  system  is  now  written  in  the  form  M{q)q  +  c(q,q)  =  B{q)u  for  which 
corollaries  4,  5  apply  directly.  The  A(q)  and  B(q)  matrices  are: 

-M2  —  sin(0) 

0  cos(9) 

0  0 

cos(0 )  0 

Although  the  algorithms  presented  in  section  2  are  defined  only  in  R3,  by 
close  inspection  of  A  and  B  one  realizes  that,  for  this  particular  example,  by 
choosing  a  R4  navigation  function  defined  only  by  the  first  three  parameters 
of  q  we  will  obtain  the  “same”  controller  has  in  R3.  Let  <p  be  a  navigation 
function  such  that  Vip=  [yx,  (py,(pg,0]T.  Next  compute  the  R4  cross  product 
of  A  and  Vy>: 

4 

x(A,Vy>)=  ^2  tijkrfVjAkiAnei  (42) 

i,j,k,l= 0 

ipe  cos(9) 

_l  <Pe  sin(0) 

=  cos(0)  -V*  cos(61)  -  Vy  sin(0) 

I  +  mR2  a 
L  R  u 

where  tijki  denotes  the  permutation  tensor,  it  are  the  canonical  basis  vectors 
and  Aj_j  is  the  itli  row,  jth  column  of  A.  We  now  compare  with  the  function 
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/2,  defined  in  equation  (11)  for  the  M3  unicycle  with  A3  =  [—  sin(0),  cos(0),  0]T 
and  Vv?3  ==  [‘Px,<Py,tPe]T'- 

fl  =  A3  X  V(/?3  = 

(fie  cos  (9) 

=  (fig  sin(0) 

— (px  cos (9)  —  (fiy  sin(0) 

The  two  previous  computations  produce,  in  effect,  the  same  behavior  for 
the  variables  x,  y  and  9.  For  each  fixed  coordinate  <f>  in  Q  C  M4  one  obtains  a 
copy  of  the  topology  of  SE(2).  Therefore,  from  here  on,  although  the  config¬ 
uration  space  is  defined  in  R4,  we  will  only  be  interested  in  x,  y  and  9. 

4.1  Navigation  Function 

Kantor  and  Rizzi  [28]  solved  the  problem  of  positioning  a  robot  in  relation  to 
a  single  engineered  beacon  by  using  the  notion  of  Sequential  Composition  of 
Controllers  [29].  The  final  approach  to  the  goal  is  implemented  using  Ikeda’s 
Variable  Constraint  Control.  We  recast  the  problem  with  a  NF-encoding  ac¬ 
cording  to  the  approach  described  in  the  previous  sections,  to  recover  in  sim¬ 
ulation  behavior  comparable  to  that  obtained  in  [28] . 

Let  h  be  a  change  of  coordinates  from  SE(2)  x  S'1  to  double  polar  coordi¬ 
nates  times  S’1  that  we  denote  by  V  with  coordinates  p  =  [77,  /z,  d,  (j)]T: 


(44) 

(45) 


V 

arctan  ( y/x ) 

d 

d 

=  h(x,y,e,<t>)  = 

9  —  arctan  (y/x) 
\Jx2  +y2 

_4> 

(46) 


Obstacles  are  introduced  on  the  field  of  view  so  that  the  robot  maintains 
a  range  of  distances  to  the  beacon  and  keeps  facing  it: 


Mm  A  9  A  fiM  S  <  d  <C  C?M 

Consider  the  following  potential  function: 


(fi{p)  := 


(2  —  cos(?7  —  77*)  —  cos(y  —  p,*)  +  (d  —  d*)2)1" 
(1  -  COs(/i  -  Mm))(l  -  COs(fi  -  fiM)) 

1 


{d  dnl  ]  (d\f  d) 

and  its  “squashed”  navigation  function  version  <p  :  V  — >  [0,1]: 

‘ pip y 


(fi(p)  := 


e  +  (fi(p)1 


(47) 


(48) 


(49) 
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The  navigation  function  written  in  the  Q  coordinates  is  <p(q)  =  (p  o  h(q ) 
and  its  derivative: 


Vt/?(g)  =  DhT (q)  •  V ip  o  h(q)  (50) 

We  choose  to  present  the  Kantor-Rizzi  example  as  the  canonical  illustra¬ 
tion  of  our  ideas  due  to  the  interesting  topology  of  the  configuration  space. 
Since  Q  is  not  simply  connected  the  level  sets  of  <p  change  from  topological 
spheres  close  to  the  goal  q*  to  topological  tori  close  to  the  boundary  of  Q. 
Initial  conditions  stating  in  the  tori  will  generate  quasi-periodic  orbits  when 
f2  is  used.  In  the  dynamical  setting  this  provides  a  good  example  of  the  ap¬ 
plicability  of  corollary  5,  resulting  in  the  generation  of  reference  dynamics 
that  attract  to  a  particular  level  set. 

4.2  Kinematic  Rolling  Disk 

We  first  simulate  the  previously  described  system  in  a  kinematic  setting  by 
solving  the  system: 

q  =  B(q)u,  (51) 

and  using  the  control  functions  /i,  /2  defined  in  section  2: 

Ui(q)  :=  h(q)  =  -HV<p  (52) 

u2(q)  :=  cr(q)f2(q)  =  x(A,Vp)a  (53) 


Figure  4  illustrates  the  resulting  simulation  where  the  initial  condition  is 
qo  =  [1, 1,  —  ^p0]T,  the  desired  goal  is  go  =  [0,  —2,  ?j,0]T,  the  body  parame¬ 
ters  are  I  =  J  =  m=  R=  1,  the  obstacles  are  pm  =  —  /zm  =  f ;  dm  =  1; 
<1m  =  3  and  a(q)  =  x.  The  manifold  {q  £  Q  :  x  =  0}  is  a  good  local  approxi¬ 
mation  for  the  stable  manifold  >VS  of  the  system  q  =  fi(q).  One  can  observe 
that  from  the  initial  time  to  ts  the  controller  f2  keeps  the  energy  constant 
while  moving  exactly  in  the  level  set  ^-1  [<£*],  with  ip*  =  0.98.  At  time  ts  we 
switch  to  controller  fi  and  the  resulting  final  position  is  very  close  to  the  goal. 
Looking  at  (f>  in  the  “positions”  graphic  one  observes  that  the  robot  does  a 
back  and  forward  motion,  necessary  to  the  parallel  park  maneuver.  This  comes 
as  a  natural  consequence  of  moving  in  the  surface  of  the  torus  shown  in  the 
“trajectories”  plot. 

4.3  Dynamic  Rolling  Disk 

For  the  dynamic  setting  we  solve  the  system  defined  by  equations  (38)  and 
(40): 


M(q)q+  c(g,g)  =  H(q)u, 


(54) 
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positions  energy 


Fig.  4.  Kinematic  simulation  of  the  vertical  rolling  disk. 

with  control  functions  (35)  and  (37): 

ui(q,  q)  ■■=  ~K2q  -Vip  (55) 

u2(q,  q)  :=  ~K2F  -  DpT  +  MDf2q  +  Cf2,  (56) 

where  F(q,  q)  :=  q  —  f2(q)  and  /i  :=  a(ip  —  p*)2  ■ 

The  first  simulation,  depicted  in  Figure  5,  uses  a  high  gain  a  =  5000  in  the 
function  /i  to  track  the  level  set  as  close  as  possible  while  the  controller  f2  is  in 
use.  That  results  in  a  good  tracking  but  very  jerky  steering  motion,  visible  in 
the  first  part  of  the  “velocities”  and  “trajectories”  plots.  The  damping  matrix 
K2  is  set  to  the  identity  matrix,  resulting  in  low  damping,  as  observed  in  the 
intervals  [ts,tf]  of  the  “positions”  and  “velocities”  plots. 

For  the  second  simulation  in  the  dynamic  setting,  depicted  in  Figure  6, 
the  parameter  a  =  250  provokes  a  less  accurate  tracking  of  the  desired  level 
set  ip* ,  when  using  f2,  as  one  can  observe  in  the  “energy”  and  “trajectories” 
plots.  However,  the  resulting  motion  is  smoother  then  the  previous  simulation. 
For  the  controller  /i,  the  damping  matrix  K2  =  10/  slows  down  the  approach 
to  the  desired  goal,  elimination  any  oscillations  as  seen  in  the  “energy”  plot. 

The  damping  matrix  K2 ,  and  the  Lyapunov  function  fi  are  the  design 
parameters  for  the  control  of  equation  (54). 

5  Conclusions 

This  exploratory  discussion  paper  addresses  the  reuse  of  navigation  functions 
developed  for  fully  actuated  bodies  in  the  setting  of  nonholonomic  constrained 
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Fig.  5.  Dynamic  simulation  of  the  vertical  rolling  disk. 


positions  velocities 
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Fig.  6.  Dynamic  simulation  of  the  vertical  rolling  disk. 
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systems  for  both  kinematic  and  dynamic  versions  of  the  model.  We  suggest 
how  the  vector  fields  developed  for  kinematic  systems  can  be  lifted  to  the 
dynamic  setting  with  the  introduction  of  damping  and  proportional  gain  type 
constants.  The  simulations  suggest  this  lifting  can  be  readily  realized  in  real 
applications,  by  proper  choice  of  the  damping  and  gain. 
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